import pybullet as p
import pybullet_data
import time

p.connect(p.GUI)
p.setGravity(0, 0, -10)

# 创建地面
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)

# 加载机械臂
robot_id = p.loadURDF("./tests/urdf/simple_arm.urdf", [0, 0, 0.1])

# 获取关节信息
num_joints = p.getNumJoints(robot_id)
print(f"机械臂有 {num_joints} 个关节")



# for i in range(num_joints):
#     info = p.getJointInfo(robot_id, i)
#     print(info)


# 控制第一个关节
p.setJointMotorControl2(
    bodyUniqueId=robot_id,
    jointIndex=0,
    controlMode=p.VELOCITY_CONTROL,
    force=0
)

# 控制第二个关节
p.setJointMotorControl2(
    bodyUniqueId=robot_id,
    jointIndex=1,
    controlMode=p.VELOCITY_CONTROL,
    force=0
)


# print(p.getDebugVisualizerCamera())
for i in p.getDebugVisualizerCamera():
    print(i)
# p.createConstraint(parentBodyUniqueId=robot_id, parentLinkIndex=0, -1, -1, jointType=p.JOINT_FIXED, [0,0,0],[0,0,0],[0,0,0])
p.createConstraint(parentBodyUniqueId=robot_id, parentLinkIndex=-1, childBodyUniqueId=-1,childLinkIndex=-1,jointType=p.JOINT_FIXED, jointAxis=[0,0,0], parentFramePosition=[0,0,-0.05], childFramePosition=[0,0,1])
# 仿真循环
for i in range(50000):
    p.stepSimulation()
    time.sleep(1./240.)

p.disconnect()